#include "../include/talker.hpp"

namespace wf
{
    namespace talker
    {   
        TalkerNode::TalkerNode(const rclcpp::NodeOptions &options) : rclcpp::Node("wf_task1_talker_node", options) 
        {
            // init
            std::cout << "starting wf_task1_talker_node..." << std::endl;
            init();

            // publish all
            std::cout << "publishing" << std::endl;
            auto t = std::make_unique<std::thread>(&TalkerNode::publish_all, this, 2.0);
            t->join();
        }

        void TalkerNode::init() 
        {
            // read parameters
            std::string dataset_dir = declare_parameter<std::string>("dataset_dir", "not declared");
            std::string image_name = declare_parameter<std::string>("image_name", "not declared");
            std::string pcd_name = declare_parameter<std::string>("pcd_name", "not declared");
            source_frame_ = declare_parameter<std::string>("source_frame", "not declared");
            target_frame_ = declare_parameter<std::string>("target_frame", "not declared");
            transform_ = declare_parameter<std::vector<double>>("transform", {});
            pose_ = declare_parameter<std::vector<double>>("pose", {});
            image_path_ = dataset_dir + "/" + image_name;
            pcd_path_ = dataset_dir + "/" + pcd_name;

            // load files
            transform_stamped_ = load_tf(transform_, source_frame_, target_frame_);
            image_ = load_image(image_path_, target_frame_);
            pcd2_ = load_pointcloud2(pcd_path_, target_frame_);
            pose_stamped_ = load_pose(pose_, source_frame_);

            // create publishers
            static_tf_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
            image_publisher_ = this->create_publisher<Image>("wf_image_raw", 1);
            pcd2_publisher_ = this->create_publisher<PointCloud2>("wf_pcd2_raw", 1);
            pose_publisher_ = this->create_publisher<PoseStamped>("wf_pose_raw", 1);
        }

        void TalkerNode::publish_all(const double rate)
        {
            // broadcast tf
            broadcast_static_tf(static_tf_broadcaster_, transform_stamped_);

            // publish others
            rclcpp::Rate rcl_rate(rate);
            while (rclcpp::ok) 
            {
                publish(image_publisher_, image_);
                publish(pcd2_publisher_, pcd2_);
                publish(pose_publisher_, pose_stamped_);
                rcl_rate.sleep();
            }
        }

        TransformStamped TalkerNode::load_tf(const std::vector<double> &transform, const std::string frame_id, const std::string child_frame_id) 
        {
            std::cout << "load tf" << std::endl;
            TransformStamped transform_stamped;
            transform_stamped.header.stamp = this->now();
            transform_stamped.header.frame_id = source_frame_;
            transform_stamped.child_frame_id = target_frame_;
            transform_stamped.transform.translation.x = transform_[0];
            transform_stamped.transform.translation.y = transform_[1];
            transform_stamped.transform.translation.z = transform_[2];
            tf2::Quaternion q;
            q.setRPY(transform_[3], transform_[4], transform_[5]);
            transform_stamped.transform.rotation = tf2::toMsg(q);
            return transform_stamped;
        }

        Image TalkerNode::load_image(std::string path, std::string frame_id)
        {
            std::cout << "load image from " + path << std::endl;
            cv::Mat img_mat = cv::imread(path);
            Image img_msg;
            cv_bridge::CvImage img_bridge {img_msg.header, "bgr8", img_mat};
            img_bridge.toImageMsg(img_msg);
            img_msg.header.frame_id = frame_id;
            return img_msg;
        }

        PointCloud2 TalkerNode::load_pointcloud2(std::string path, std::string frame_id)
        {
            std::cout << "load pointcloud from " + path << std::endl;
            pcl::PointCloud<pcl::PointXYZI>::Ptr pcd;
            pcd.reset(new pcl::PointCloud<pcl::PointXYZI>());
            pcl::io::loadPCDFile(path, *pcd);
            PointCloud2 pcd2_msg;
            pcl::toROSMsg(*pcd, pcd2_msg);
            pcd2_msg.header.frame_id = frame_id;
            return pcd2_msg;
        }

        PoseStamped TalkerNode::load_pose(const std::vector<double> &pose, const std::string frame_id) 
        {
            std::cout << "load pose" << std::endl;
            PoseStamped pose_stamped;
            pose_stamped.pose.position.x = pose[0];
            pose_stamped.pose.position.y = pose[1];
            pose_stamped.pose.position.z = pose[2];
            tf2::Quaternion q;
            q.setRPY(pose[3], pose[4], pose[5]);
            pose_stamped.pose.orientation = tf2::toMsg(q);
            return pose_stamped;
        }

        void TalkerNode::broadcast_static_tf(std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster, TransformStamped &tf) 
        {
            tf.header.stamp = this->now();
            static_tf_broadcaster->sendTransform(tf);
        }

        template <typename T>
        void TalkerNode::publish(typename rclcpp::Publisher<T>::SharedPtr publisher, T &message)
        {
            message.header.stamp = this->now();
            publisher->publish(message);
        }

    } // namespace talker
    
} // namespace wf

// register node
RCLCPP_COMPONENTS_REGISTER_NODE(wf::talker::TalkerNode)
